#include "device.h"


/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */


void solvde(  int itmax,  	/* maximum number of iterations */
	      double conv,	/* convergence criterion	*/
	      double slowc,	/* controls the fraction of corrections actually 
				   used after each iteration*/
	      double scalv[],	/* scalv[1..ne] contains typical sizes for each 
				   dependent variable, used to weight errors */
	      int indexv[],	/* indexv[1..ne] lists the column ordering of 
				   variables used to construct the matrix 
				   s[1..ne][1..2*ne+1] of derivatives. */
	      int ne,	        /* ne equations for ne adjustable dependent variables */
	      int nb,		/* nb boundary conditions */
	      int m,		/* There are a total of m mesh points. */
	      double **y,	/* y[1..ne][1..m] Contains the initial guess for all the 
				   dependent variables at each mesh point */
	      double ***c,	/* c[1..ne][1..ne-nb+1][1..m+1]dummy storage */
	      double **s,	/* s[1..ne][1..2*ne+1] matrix of derivatives */	
	      Device & dev) {

/* Driver routine for solution of two point boundary value problems by relaxation.
   itmax is the maximum number of iterations. 
   conv is the convergence criterion. 
   slowc controls the fraction of corrections actually used after each iteration.
   scalv[1..ne] contains typical sizes for each dependent variable, used to weight errors.
   indexv[1..ne] lists the column ordering of variables used to construct 
   the matrix s[1..ne][1..2*ne+1] of derivatives. 
   (The nb boundary conditions at the first mesh point must contain some dependence 
   on the first nb variables listed in indexv.) The problem involves ne equations 
   for ne adjustable dependent variables at each point. At the first mesh point 
   there are nb boundary conditions. There are a total of m mesh points. 
   y[1..ne][1..m] is the two-dimensional array that contains the initial guess for 
   all the dependent variables at each mesh point. On each iteration, it is updated 
   by the calculated correction. 
   The arrays c[1..ne][1..ne-nb+1][1..m+1] and s supply dummy storage used by the 
   relaxation code.	*/
  
  int ic1,ic2,ic3,ic4,it,j,j1,j2,j3,j4,j5,j6,j7,j8,j9; 
  int jc1,jcf,jv,k,k1,k2,km,kp,nvars,*kmax; 
  double err,errj,fac,vmax,vz,*ermax; 
  
  
  kmax=ivector(1,ne); 
  ermax=vector(1,ne);
  k1=1; /*Set up row and column markers.*/
  k2=m; 
  nvars=ne*m; 
  j1=1; 
  j2=nb; 
  j3=nb+1; 
  j4=ne; 
  j5=j4+j1; 
  j6=j4+j2; 
  j7=j4+j3; 
  j8=j4+j4; 
  j9=j8+j1; 
  ic1=1; 
  ic2=ne-nb; 
  ic3=ic2+1; 
  ic4=ne; 
  jc1=1; 
  jcf=ic3;
  for (it=1;it<=itmax;it++) { /*Primary iteration loop.*/
    k=k1; /*Boundary conditions at first point.*/
    dev.difeq(k,k1,k2,j9,ic3,ic4,indexv,ne,s,y);
    pinvs(ic3,ic4,j5,j9,jc1,k1,c,s);
    for(k=k1+1;k<=k2;k++) { /*Finite difference equations at all point pairs.*/
      kp=k-1; 
      dev.difeq(k,k1,k2,j9,ic1,ic4,indexv,ne,s,y);
      red(ic1,ic4,j1,j2,j3,j4,j9,ic3,jc1,jcf,kp,c,s);
      pinvs(ic1,ic4,j3,j9,jc1,k,c,s);
    } 
    k=k2+1; /*Final boundary conditions.*/
    dev.difeq(k,k1,k2,j9,ic1,ic2,indexv,ne,s,y);
    red(ic1,ic2,j5,j6,j7,j8,j9,ic3,jc1,jcf,k2,c,s);
    pinvs(ic1,ic2,j7,j9,jcf,k2+1,c,s);
    bksub(ne,nb,jcf,k1,k2,c); /*Backsubstitution.*/
    err=0.0; 
    for (j=1;j<=ne;j++) { /*Convergence check, accumulate average error.*/
      jv=indexv[j]; 
      errj=vmax=0.0; 
      km=0; 
      for(k=k1;k<=k2;k++) { /*Find point with largest error, for each dependent variable.*/
	vz=fabs(c[jv][1][k]); 
	if(vz > vmax) { 
	  vmax=vz;
	  km=k; 
	}
	errj += vz;
      } 
      err += errj/scalv[j]; /*Note weighting for each dependent variable.*/
      ermax[j]=c[jv][1][km]/scalv[j];
      kmax[j]=km;
    } 
    err /= nvars;
    fac=(err > slowc ? slowc/err : 1.0); /*Reduce correction applied when error is large.*/
    for (j=1;j<=ne;j++) { /*Apply corrections.*/
      jv=indexv[j];
      for(k=k1;k<=k2;k++) y[j][k] -= fac*c[jv][1][k]; 
    } 
    // printf("\n%8s %9s %9s\n","Iter.","Error","FAC"); // Summary of corrections for this step. 
    // if((it%10)==0) printf("Iter = %6d   Error = %20.12lf   FAC = %11.6lf\n",it,err,fac);
    if (err < conv) { /*Point with largest error for each variable can be monitored by writing out kmax and ermax.*/
      free_vector(ermax,1,ne); 
      free_ivector(kmax,1,ne);
      return; 
    } 
  } 
  printf("Too many iterations in solvde\n"); /*Convergence failed.*/
  return;
}

/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */

void bksub(int ne, int nb, int jf, int k1, int k2, double ***c) {

/*	Backsubstitution, used internally by solvde.*/

	int nbf,im,kp,k,j,i;
	double xx; 

	nbf=ne-nb; 
	im=1; 
	for (k=k2;k>=k1;k--) { /*Use recurrence relations to eliminate remaining dependences.*/
 		if (k == k1) im=nbf+1; 
		kp=k+1; 
		for (j=1;j<=nbf;j++) { 
			xx=c[j][jf][kp]; 
			for (i=im;i<=ne;i++) c[i][jf][k] -= c[i][j][k]*xx; 
		} 
	} 
	for (k=k1;k<=k2;k++) { /*Reorder corrections to be in column 1.*/
 		kp=k+1; 
		for (i=1;i<=nb;i++) c[i][1][k]=c[i+nbf][jf][k]; 
		for (i=1;i<=nbf;i++) c[i+nb][1][k]=c[i][jf][kp]; 
	}
 
}

/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */

void pinvs(int ie1, int ie2, int je1, int jsf, int jc1, int k, double ***c, double **s) {

/* Diagonalize the square subsection of the s matrix, and store the recursion 
	coefficients in c; used internally by solvde. */

	int js1,jpiv,jp,je2,jcoff,j,irow,ipiv,id,icoff,i,*indxr,ii;
	double pivinv,piv,dum,big,*pscl; 

	indxr=ivector(ie1,ie2); 
	pscl=vector(ie1,ie2); 
	je2=je1+ie2-ie1; 
	js1=je2+1;
	for (i=ie1;i<=ie2;i++) { /*Implicit pivoting */
		big=0.0; 
		for (j=je1;j<=je2;j++) if (fabs(s[i][j]) > big) big=fabs(s[i][j]); 
		if (big == 0.0) {
			printf("ie1 =%d  ie2 =%d  je1 =%d  je2 =%d\n  ",ie1,ie2,je1,je2);
			for(i=1;i<=NE;i++)	{
				for(ii=1;ii<=2*NE+1;ii++) printf("%le  ",s[i][ii]);
				printf("\n");
			}
			nrerror("XXSingular matrix - row all 0, in pinvs");
		} 
		pscl[i]=1.0/big; 
		indxr[i]=0; 
	} 
	for (id=ie1;id<=ie2;id++) {
		piv=0.0; 
		for (i=ie1;i<=ie2;i++) { /*Find pivot element.*/
 			if (indxr[i] == 0) { 
				big=0.0; 
				for (j=je1;j<=je2;j++) { 
					if (fabs(s[i][j]) > big) { 
						jp=j; 
						big=fabs(s[i][j]); 
					} 
				} 
				if (big*pscl[i] > piv) { 
					ipiv=i; 
					jpiv=jp; 
					piv=big*pscl[i]; 
				} 
			} 
		} 
		if (s[ipiv][jpiv] == 0.0) nrerror("Singular matrix in routine pinvs"); 
		indxr[ipiv]=jpiv; /*In place reduction. Save column ordering.*/
 		pivinv=1.0/s[ipiv][jpiv]; 
		for (j=je1;j<=jsf;j++) s[ipiv][j] *= pivinv; /*Normalize pivot row.*/ 
		s[ipiv][jpiv]=1.0; 
		for (i=ie1;i<=ie2;i++) { /*Reduce nonpivot elements in column.*/ 
			if (indxr[i] != jpiv) { 
				if (s[i][jpiv]) { 
					dum=s[i][jpiv]; 
					for (j=je1;j<=jsf;j++) s[i][j] -= dum*s[ipiv][j]; 
					s[i][jpiv]=0.0; 
				} 
			} 
		} 
	} 
	jcoff=jc1-js1;/* Sort and store unreduced coefficients.*/
 	icoff=ie1-je1;
	for (i=ie1;i<=ie2;i++) { 
		irow=indxr[i]+icoff; 
		for (j=js1;j<=jsf;j++) c[irow][j+jcoff][k]=s[i][j]; 
	} 
	free_vector(pscl,ie1,ie2); 
	free_ivector(indxr,ie1,ie2); 

} 


/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */


void red(int iz1, int iz2, int jz1, int jz2, int jm1, int jm2, int jmf, int ic1, int jc1, int jcf, int kc, double ***c, double **s) {

/*	Reduce columns jz1-jz2 of the s matrix, using previous results as stored 
	in the c matrix. Only columns jm1-jm2,jmf are affected by the prior results.
	red is used internally by solvde. */

	int loff,l,j,ic,i; 
	double vx; 
	
	loff=jc1-jm1; 
	ic=ic1;
	for (j=jz1;j<=jz2;j++) { /*Loop over columns to be zeroed.*/
 		for (l=jm1;l<=jm2;l++) { /*Loop over columns altered.*/ 
			vx=c[ic][l+loff][kc]; 
			for (i=iz1;i<=iz2;i++) s[i][l] -= s[i][j]*vx; /*Loop over rows.*/ 
		} 
		vx=c[ic][jcf][kc]; 
		for (i=iz1;i<=iz2;i++) s[i][jmf] -= s[i][j]*vx; /*Plus final element.*/
 		ic += 1; 
	} 

}


/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */
